# motions implements the motionInterface
from naoqi import ALProxy
import time
import math

track = ALProxy("ALRedBallTracker", "localhost", 9559)
motion = ALProxy("ALMotion", "localhost", 9559)
pose = ALProxy("ALRobotPose", "localhost", 9559)
video = ALProxy("ALVideoDevice", "localhost", 9559)

track.setWholeBodyOn(False)

def changeHead((yaw,pitch)):
	motion.changeAngles(["HeadYaw", "HeadPitch"], [yaw, pitch], 0.3)

def setHead((yaw, pitch)):
	motion.setAngles(["HeadYaw", "HeadPitch"], [yaw, pitch], 0.4)

def stiff():
	motion.setStiffnesses("Body", 0.8)
	# motion.setStiffnesses("HeadPitch", 0)

def normalHead():
	motion.setAngles("Head", [0,-0.5], 0.3)
	
def normalPose():
	names = list()
	times = list()
	keys = list()
	
	if not(track.isActive()):
		names.append("HeadPitch")
		times.append([1.5])
		keys.append([ [ -0.5 , [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
		
		names.append("HeadYaw")
		times.append([1.5])
		keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("LShoulderPitch")
	times.append([1.5])
	keys.append([ [ 1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LShoulderRoll")
	times.append([1.5])
	keys.append([ [ 0.15, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowYaw")
	times.append([1.5])
	keys.append([ [ -0.5, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowRoll")
	times.append([1.5])
	keys.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RShoulderPitch")
	times.append([1.5])
	keys.append([ [ 1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RShoulderRoll")
	times.append([1.5])
	keys.append([ [ -0.15, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowYaw")
	times.append([1.5])
	keys.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowRoll")
	times.append([1.5])
	keys.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipYawPitch")
	times.append([1.5])
	keys.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipRoll")
	times.append([1.5])
	keys.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipPitch")
	times.append([1.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LKneePitch")
	times.append([1.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LAnklePitch")
	times.append([1.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LAnkleRoll")
	times.append([1.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipRoll")
	times.append([1.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipPitch")
	times.append([1.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RKneePitch")
	times.append([1.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnklePitch")
	times.append([1.5])
	keys.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnkleRoll")
	times.append([1.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	motion.angleInterpolationBezier(names, times, keys)	
	
def postWalkTo((x, y, angle)):
	motion.post.walkTo(x, y, angle)	

def walkTo((x,y,angle)):
	motion.walkTo(x,y,angle)
	
def scanMove(yaw):
	# Put head straight
	track.getPosition()
	if not(track.isActive()):
		track.startTracker()
	
	angles = [[-yaw, 0.4], [-yaw/2.0, 0.4], [0, 0.5], [yaw/2.0, 0.4], [yaw, 0.4],\
 			  [yaw, 0.2], [yaw/2.0 , 0.2], [0, 0.2], [-yaw/2.0, 0.2], [-yaw, 0.2]]
	
	for i in range(len(angles)):
		motion.post.setAngles( ["HeadYaw", "HeadPitch"], angles[i], 0.9)
		startTime = time.time()
		while time.time() - startTime < 0.7:
			if track.isNewData():
				return True			
	return False
	
def rKickAngled(angle):
	motion.setAngles(["LShoulderRoll", "LShoulderPitch", "RShoulderRoll", "RShoulderPitch"], [0.5 + 0.3*angle, 0.8, -0.3, 0.8], 0.3)
	# Turn on left foot
	motion.setAngles(["LAnkleRoll", "RAnkleRoll"], [0.21 + 0.005 * angle, 0.2], 0.2)	
	time.sleep(0.3)
	# Left leg stretched, right leg goes backwards
	motion.angleInterpolation(["LHipYawPitch", "LHipRoll", "LKneePitch", "LAnklePitch", "RHipRoll", "LHipPitch", "RHipPitch", "RKneePitch", "RAnklePitch"], [[0.65*angle], [0.2*angle], [0.2 + 0.3*angle], [-0.2 -0.2*angle], [0.075 -0.35*angle], [-0.05 - 0.15*angle],  [0.4], [0.5 - 0.35*angle], [-0.6 + 0.5*angle]], [[0.6],[0.6],[0.6],[0.6],[0.6],[0.6],[0.6],[0.7],[0.7]], True)
	
	# motion.setAngles([                                                                            "LHipPitch", "RHipPitch", "RKneePitch", "RAnklePitch"], [                                                                   -0.05,            0.4,     0.3,            -0.65],           0.2)
	time.sleep(0.3)
	# kick
	motion.setAngles("RShoulderPitch", 2, 1)
	motion.setAngles(["LHipYawPitch", "LHipRoll", "LHipPitch", "RHipPitch", "RKneePitch", "RAnklePitch"], [0.75*angle, 0.1, 0.05 -0.3*angle , -0.4 * angle, 0, 0.15 - 0.35*angle], 0.9)
	# motion.setAngles(["RShoulderPitch",                           "LHipPitch", "RHipPitch", "RKneePitch", "RAnklePitch"], [1.6,                       0.05,        -0.4, 0, 0.45],          0.9)
	time.sleep(0.4)
	# return to start position
	motion.angleInterpolation(["LHipYawPitch", "LHipRoll", "RHipRoll", "LHipPitch", "RHipPitch", "LKneePitch", "RKneePitch", "LAnkleRoll", "RAnkleRoll", "LAnklePitch", "RAnklePitch"], [[0], [0], [0], [0], [0], [0], [0], [0.2], [0], [-0.05], [0]], [[0.55],[0.65],[0.55],[0.65],[0.65],[0.55],[0.65],[0.65],[0.65],[0.55],[0.65]], True)
	time.sleep(0.3 * angle)
	motion.setAngles(["LShoulderRoll", "LShoulderPitch", "RShoulderRoll", "RShoulderPitch"], [0, 1.2, 0, 1.2], 0.15)
	motion.setAngles("LAnklePitch", 0, 0.025)
	time.sleep(0.4)
	motion.setAngles(["LAnkleRoll", "RAnkleRoll"], [0, 0], 0.05)

def lKickAngled(angle):
	motion.setAngles(["RShoulderRoll", "RShoulderPitch", "LShoulderRoll", "LShoulderPitch"], [-0.5 - 0.3*angle, 0.8, 0.3, 0.8], 0.3)
	# Turn on right foot
	motion.setAngles(["RAnkleRoll", "LAnkleRoll"], [-0.21 - 0.014 * angle, -0.2], 0.2)	
	time.sleep(0.3)
	# Right leg stretched, left leg goes backwards
	motion.angleInterpolation(["LHipYawPitch", "RHipRoll", "RKneePitch", "RAnklePitch", "LHipRoll", "RHipPitch", "LHipPitch", "LKneePitch", "LAnklePitch"], [[0.65*angle], [-0.2*angle], [0.2 + 0.3*angle], [-0.2 -0.2*angle], [-0.075 + 0.35*angle], [-0.05 - 0.15*angle],  [0.4], [0.5 - 0.35*angle], [-0.6 + 0.5*angle]], [[0.6],[0.6],[0.6],[0.6],[0.6],[0.6],[0.6],[0.7],[0.7]], True)
	# motion.setAngles([                                                                            "RHipPitch", "LHipPitch", "LKneePitch", "LAnklePitch"], [                                                                      -0.05,            0.4,     0.3,            -0.65],           0.2)
	time.sleep(0.3)
	# kick
	motion.setAngles("LShoulderPitch", 2, 1)
	motion.setAngles(["LHipYawPitch", "RHipRoll", "RHipPitch", "LHipPitch", "LKneePitch", "LAnklePitch"], [0.75*angle, -0.1, 0.05 -0.3*angle , -0.4 * angle, 0, 0.15 - 0.35*angle], 0.9)
	# motion.setAngles(["LShoulderPitch",                           "RHipPitch", "LHipPitch", "LKneePitch", "LAnklePitch"], [1.6,                       0.05,        -0.4, 0, 0.45],          0.9)
	time.sleep(0.4)
	# return to start position
	motion.angleInterpolation(["LHipYawPitch", "RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKneePitch", "LKneePitch", "RAnkleRoll", "LAnkleRoll", "RAnklePitch", "LAnklePitch"], [[0], [0], [0], [0], [0], [0], [0], [-0.2], [0], [-0.05], [0]], [[0.55],[0.65],[0.55],[0.65],[0.65],[0.55],[0.65],[0.65],[0.65],[0.55],[0.65]], True)
	time.sleep(0.3 * angle)
	motion.setAngles(["RShoulderRoll", "RShoulderPitch", "LShoulderRoll", "LShoulderPitch"], [0, 1.2, 0, 1.2], 0.15)
	motion.setAngles("RAnklePitch", 0, 0.025)
	time.sleep(0.4)
	motion.setAngles(["RAnkleRoll", "LAnkleRoll"], [0, 0], 0.05)

def hakje():
	motion.setAngles(["RAnkleRoll", "LAnkleRoll"], [-0.2, -0.2], 0.2)
	time.sleep(0.3)
	# step
	motion.angleInterpolation(["LHipPitch", "LKneePitch", "LAnklePitch", "RHipPitch", "RKneePitch", "RAnklePitch"], [[-0.29],[0.23],[0.1],[0.22],[0],[-0.17]], [[0.6],[0.6],[0.6],[0.6],[0.6],[0.6]], True)
	time.sleep(0.4)	
	motion.setAngles(["LAnkleRoll", "RAnkleRoll"], [0, 0], 0.1)	

	# stand	
	motion.angleInterpolation(["LHipPitch", "LKneePitch", "LAnklePitch", "RHipPitch", "RKneePitch", "RAnklePitch"], [[-0.2],[0.1],[0.1],[0.1],[0.05],[-0.15]], [[0.6],[0.6],[0.6],[0.6],[0.6],[0.6]], True)
	motion.setAngles(["LShoulderRoll", "LShoulderPitch", "RShoulderRoll", "RShoulderPitch"], [1, 0.8, -0.4, 0.8], 0.3)
	# Turn on left foot
	motion.setAngles(["LAnkleRoll", "RAnkleRoll"], [0.215, 0.2], 0.2)	
	time.sleep(0.8)
	# lift leg
	motion.angleInterpolation(["LHipPitch", "LKneePitch", "LAnklePitch", "RHipPitch", "RKneePitch", "RAnklePitch"], [[0],[0],[0],[-1],[2],[-1]], [[0.7],[0.7],[0.7],[0.9],[0.7],[0.7]], True)
	time.sleep(1)
	# leg over ball, heel
	motion.angleInterpolation(["RHipRoll", "RHipPitch", "RKneePitch", "RAnklePitch"], [[-0.5, 0], [-1.2, -0.4], [0.2, 0.4], [0.6, -0.3]], [[0.3, 0.6], [0.7, 0.8], [0.7, 0.9], [0.7, 0.8]], True)
	motion.setAngles(["RHipPitch", "RKneePitch", "RAnklePitch", "LAnkleRoll", "RAnkleRoll", "LShoulderRoll", "RShoulderRoll", "LShoulderPitch", "RShoulderPitch"], [0,0,0, 0,0, 0.1, -0.1, 1.5, 1.5], 0.1)	
	
def footLeft():
	names = list()
	times = list()
	keys = list()
	
	names.append("RShoulderPitch")
	times.append([1])
	keys.append([ [1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RShoulderRoll")
	times.append([1])
	keys.append([ [-0.43, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowYaw")
	times.append([1])
	keys.append([ [-0.61, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowRoll")
	times.append([1])
	keys.append([ [0.44, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipYawPitch")
	times.append([0.75])
	keys.append([ [-0.98, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipRoll")
	times.append([1])
	keys.append([ [0.1426, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipPitch")
	times.append([1])
	keys.append([ [-0.2222, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RKneePitch")
	times.append([1])
	keys.append([ [2.11, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("RAnklePitch")
	times.append([1])
	keys.append([ [-1, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("RAnkleRoll")
	times.append([1])
	keys.append([ [0.079, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipRoll")
	times.append([1])
	keys.append([ [0.68, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipPitch")
	times.append([0.75])
	keys.append([ [-1.4, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LKneePitch")
	times.append([1])
	keys.append([ [0.8, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LAnklePitch")
	times.append([1])
	keys.append([ [1, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LAnkleRoll")
	times.append([1])
	keys.append([ [-0.05, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])	
	
	names.append("LShoulderPitch")
	times.append([1])
	keys.append([ [1.56, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LShoulderRoll")
	times.append([1])
	keys.append([ [0.78, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowYaw")
	times.append([1])
	keys.append([ [0.3, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowRoll")
	times.append([1])
	keys.append([ [0.05, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	motion.angleInterpolationBezier(names, times, keys);
	
	# return to original state
	names = list()
	times = list()
	keys = list()
	
	names.append("RShoulderPitch")
	times.append([0.5, 6])
	keys.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]], [1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RShoulderRoll")
	times.append([2.5])
	keys.append([ [ -0.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowYaw")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowRoll")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipYawPitch")
	times.append([4])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipRoll")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipPitch")
	times.append([2.5])
	keys.append([ [ -1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RKneePitch")
	times.append([3])
	keys.append([ [ 1.8, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("RAnklePitch")
	times.append([3])
	keys.append([ [ -0.75, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("RAnkleRoll")
	times.append([3])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipRoll")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipPitch")
	times.append([4])
	keys.append([ [ -1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LKneePitch")
	times.append([3])
	keys.append([ [ 1.8, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LAnklePitch")
	times.append([4])
	keys.append([ [ -0.75, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LAnkleRoll")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])	
	
	names.append("LShoulderPitch")
	times.append([2.5])
	keys.append([ [ 1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LShoulderRoll")
	times.append([2.5])
	keys.append([ [ 0.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowYaw")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowRoll")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	motion.angleInterpolationBezier(names, times, keys)

def footRight():
	names = list()
	times = list()
	keys = list()
	
	names.append("LShoulderPitch")
	times.append([1])
	keys.append([ [1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LShoulderRoll")
	times.append([1])
	keys.append([ [0.43, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowYaw")
	times.append([1])
	keys.append([ [-0.61, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowRoll")
	times.append([1])
	keys.append([ [-0.44, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipYawPitch")
	times.append([0.75])
	keys.append([ [-0.98, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipRoll")
	times.append([1])
	keys.append([ [-0.1426, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipPitch")
	times.append([1])
	keys.append([ [-0.2222, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LKneePitch")
	times.append([1])
	keys.append([ [2.11, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("LAnklePitch")
	times.append([1])
	keys.append([ [-1, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("LAnkleRoll")
	times.append([1])
	keys.append([ [-0.079, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipRoll")
	times.append([1])
	keys.append([ [-0.68, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipPitch")
	times.append([0.75])
	keys.append([ [-1.4, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RKneePitch")
	times.append([1])
	keys.append([ [0.8, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnklePitch")
	times.append([1])
	keys.append([ [1, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnkleRoll")
	times.append([1])
	keys.append([ [0.05, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])	
	
	names.append("RShoulderPitch")
	times.append([1])
	keys.append([ [1.56, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RShoulderRoll")
	times.append([1])
	keys.append([ [-0.78, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowYaw")
	times.append([1])
	keys.append([ [0.3, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowRoll")
	times.append([1])
	keys.append([ [-0.05, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	motion.angleInterpolationBezier(names, times, keys);
	
	# return to original state
	names = list()
	times = list()
	keys = list()
	
	names.append("LShoulderPitch")
	times.append([0.5, 6])
	keys.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]], [ 1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LShoulderRoll")
	times.append([2.5])
	keys.append([ [ 0.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowYaw")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowRoll")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipYawPitch")
	times.append([4])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipRoll")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipPitch")
	times.append([2.5])
	keys.append([ [ -1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LKneePitch")
	times.append([3])
	keys.append([ [ 1.8, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("LAnklePitch")
	times.append([3])
	keys.append([ [ -0.75, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("LAnkleRoll")
	times.append([3])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipRoll")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipPitch")
	times.append([4])
	keys.append([ [ -1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RKneePitch")
	times.append([3])
	keys.append([ [ 1.8, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnklePitch")
	times.append([4])
	keys.append([ [ -0.75, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnkleRoll")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])	
	
	names.append("RShoulderPitch")
	times.append([2.5])
	keys.append([ [ 1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RShoulderRoll")
	times.append([2.5])
	keys.append([ [ -0.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowYaw")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowRoll")
	times.append([2.5])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	motion.angleInterpolationBezier(names, times, keys);

def diveLeft():
	names = list()
	angles = list()
	times = list()
	
	names.extend(["LHipRoll", "LHipPitch", "LKneePitch", "LAnklePitch", "LAnkleRoll"])
	angles.extend([[0.438], [-0.8], [2.11], [-1.186], [-0.09]])
	times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]])
	
	names.append("LHipYawPitch")
	angles.append([0.1])
	times.append([0.5])
	
	names.extend(["RHipRoll", "RHipPitch", "RKneePitch", "RAnklePitch", "RAnkleRoll"])
	angles.extend([[0], [-0.3], [0.6871], [-0.3099], [0.54788]])
	times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]])

	names.extend(["LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll"])	
	angles.extend([[-1.1], [0.5], [0.1883], [-0.5016]])
	times.extend([[0.2], [0.2], [0.2], [0.2]])

	names.extend(["RShoulderPitch","RShoulderRoll", "RElbowYaw", "RElbowRoll"])
	angles.extend([[0.7, 0.9], [-0.11, 0.3403], [-0.1, 0.1226], [0.25, 0.903]])
	times.extend([[0.2,0.5], [0.2,0.5], [0.2,0.5], [0.2, 0.5]])
	
	motion.angleInterpolation(names, angles, times, True)
	motion.setStiffnesses("Body", 0)
	time.sleep(2)
	motion.setStiffnesses("RArm", 0.8)
	motion.setStiffnesses("LLeg", 0.6)
	motion.setStiffnesses("RLeg", 0.6)
	motion.angleInterpolation(["RShoulderPitch","RShoulderRoll", "RElbowYaw", "RElbowRoll"], [[1.372, 2.085], [-1.2916, -0.105], [-0.154, -0.130], [1.1888, 0.380]], [[0.4, 0.8], [0.4, 0.8], [0.4, 0.8], [0.4, 0.8]], True)
	motion.setAngles("RLeg", [-0.5,0,0,0.6,0.2,-0.2], 0.2)	
	motion.setAngles("LLeg", [-0.5,0,0,0.6,0.2, 0.2], 0.2)
	motion.setStiffnesses("RArm", 0)
	while pose.getActualPoseAndTime()[0] != "Back" and pose.getActualPoseAndTime()[0] != "Belly":
		pass
	motion.setStiffnesses("Body",0.8)
	if pose.getActualPoseAndTime()[0] == "Back":
		backToStand()
	elif pose.getActualPoseAndTime()[0] == "Belly":
		bellyToStand()
	motion.walkTo(0.1, 0.15, 1.5)
	
def diveRight():
	names = list()
	angles = list()
	times = list()
	
	names.extend(["RHipRoll", "RHipPitch", "RKneePitch", "RAnklePitch", "RAnkleRoll"])
	angles.extend([[-0.438], [-0.8], [2.11], [-1.186], [0.09]])
	times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]])
	
	names.append("LHipYawPitch")
	angles.append([0.1])
	times.append([0.5])
	
	names.extend(["LHipRoll", "LHipPitch", "LKneePitch", "LAnklePitch", "LAnkleRoll"])
	angles.extend([[0], [-0.3], [0.6871], [-0.3099], [-0.54788]])
	times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]])
	
	names.extend(["RShoulderPitch","RShoulderRoll", "RElbowYaw", "RElbowRoll"])
	angles.extend([[-1.1], [-0.5], [0.1883], [0.5016]])
	times.extend([[0.2], [0.2], [0.2], [0.2]])
	
	names.extend(["LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll"])
	angles.extend([[0.7, 0.9], [0.11, -0.3403], [-0.1, 0.1226], [-0.25, -0.903]])
	times.extend([[0.2,0.5], [0.2,0.5], [0.2,0.5], [0.2, 0.5]])
	
	motion.angleInterpolation(names, angles, times, True)
	motion.setStiffnesses("Body", 0)
	time.sleep(2)
	motion.setStiffnesses("LArm", 0.8)
	motion.setStiffnesses("LLeg", 0.6)
	motion.setStiffnesses("RLeg", 0.6)
	motion.angleInterpolation(["LShoulderPitch","LShoulderRoll", "LElbowYaw", "LElbowRoll"], [[1.372, 2.085], [1.2916, 0.105], [-0.154, -0.130], [-1.1888, -0.380]], [[0.4, 0.8], [0.4, 0.8], [0.4, 0.8], [0.4, 0.8]], True)
	motion.setAngles("LLeg", [-0.5,0,0,0.6,0.2, 0.2], 0.2)	
	motion.setAngles("RLeg", [-0.5,0,0,0.6,0.2,-0.2], 0.2)
	motion.setStiffnesses("RArm", 0)
	while pose.getActualPoseAndTime()[0] != "Back" and pose.getActualPoseAndTime()[0] != "Belly":
		pass	
	motion.setStiffnesses("Body",0.8)
	if pose.getActualPoseAndTime()[0] == "Back":
		backToStand()
	elif pose.getActualPoseAndTime()[0] == "Belly":
		bellyToStand()
	motion.walkTo(0.1, -0.15, -1.5)
	
# Stand up from the back
# Stand up from the back
def backToStand():
	names = list()
	times = list()
	keys = list()

	names.append("HeadYaw")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ -0.02459, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.00000, [ 3, -0.33333, -0.01222], [ 3, 0.26667, 0.00977]], [ 0.04138, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, 0.00000], [ 3, 0.23333, -0.00000]], [ 0.00000, [ 3, -0.23333, 0.00000], [ 3, 0.16667, -0.00000]], [ 0.00000, [ 3, -0.16667, 0.00000], [ 3, 0.26667, -0.00000]], [ -0.51393, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.30224, [ 3, -0.30000, -0.12958], [ 3, 0.30000, 0.12958]], [ 0.26354, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.12043, [ 3, -0.20000, 0.04549], [ 3, 0.20000, -0.04549]], [ -0.00940, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("HeadPitch")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.06285, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.51490, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.67200, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.00000, [ 3, -0.23333, -0.19854], [ 3, 0.16667, 0.14181]], [ 0.34907, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.17177, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.34511, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.01047, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.02967, [ 3, -0.20000, -0.01920], [ 3, 0.20000, 0.01920]], [ 0.37886, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.15, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LShoulderPitch")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 2.08560, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 2.09440, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.03865, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.08560, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 2.08560, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 2.08560, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.03251, [ 3, -0.26667, 0.05309], [ 3, 0.30000, -0.05973]], [ 1.28698, [ 3, -0.30000, 0.17922], [ 3, 0.30000, -0.17922]], [ 0.95717, [ 3, -0.30000, 0.11076], [ 3, 0.20000, -0.07384]], [ 0.73321, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.73321, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 1.59225, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LShoulderRoll")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.98018, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.36652, [ 3, -0.33333, 0.02182], [ 3, 0.26667, -0.01745]], [ 0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.36652, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.29909, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.07052, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.26687, [ 3, -0.30000, -0.08560], [ 3, 0.20000, 0.05706]], [ 0.49851, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.49851, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.21932, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("LElbowYaw")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ -0.76091, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.15708, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.12268, [ 3, -0.26667, 0.01164], [ 3, 0.26667, -0.01164]], [ 0.08727, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.08727, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.08727, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.96049, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.85448, [ 3, -0.30000, -0.28507], [ 3, 0.30000, 0.28507]], [ -0.25008, [ 3, -0.30000, -0.18751], [ 3, 0.20000, 0.12501]], [ 0.08308, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.08308, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ -1.03089, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowRoll")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.5, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ -0.62430, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.69813, [ 3, -0.23333, -0.30205], [ 3, 0.16667, 0.21575]], [ -0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.13333, 0.00000]], [ -0.19199, [ 3, -0.13333, 0.17291], [ 3, 0.13333, -0.17291]], [ -1.04615, [ 3, -0.13333, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.87280, [ 3, -0.30000, -0.03605], [ 3, 0.30000, 0.03605]], [ -0.82985, [ 3, -0.30000, -0.01349], [ 3, 0.20000, 0.00899]], [ -0.80535, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.80535, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ -0.68105, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RShoulderPitch")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 2.08560, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 2.09440, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.95283, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.08560, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 2.08560, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 2.08560, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.07708, [ 3, -0.26667, 0.00759], [ 3, 0.30000, -0.00854]], [ 2.03720, [ 3, -0.30000, 0.03988], [ 3, 0.30000, -0.03988]], [ 1.44047, [ 3, -0.30000, 0.22918], [ 3, 0.20000, -0.15279]], [ 0.89131, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.89131, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 1.46961, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("RShoulderRoll")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ -1.07384, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.36652, [ 3, -0.33333, -0.02182], [ 3, 0.26667, 0.01745]], [ -0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.36652, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.45717, [ 3, -0.26667, 0.08719], [ 3, 0.30000, -0.09809]], [ -0.56455, [ 3, -0.30000, 0.04142], [ 3, 0.30000, -0.04142]], [ -0.70568, [ 3, -0.30000, 0.06162], [ 3, 0.20000, -0.04108]], [ -0.87266, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.68068, [ 3, -0.20000, -0.05778], [ 3, 0.63333, 0.18298]], [ -0.15037, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowYaw")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.76542, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.15708, [ 3, -0.33333, 0.12584], [ 3, 0.26667, -0.10067]], [ -0.25775, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.08727, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.08727, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.08727, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.08134, [ 3, -0.26667, -0.00285], [ 3, 0.30000, 0.00321]], [ -0.06907, [ 3, -0.30000, -0.00920], [ 3, 0.30000, 0.00920]], [ -0.02612, [ 3, -0.30000, -0.01394], [ 3, 0.20000, 0.00929]], [ 0.00062, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.00062, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.91269, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowRoll")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.71028, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.69813, [ 3, -0.23333, 0.30205], [ 3, 0.16667, -0.21575]], [ 0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.09668, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.02152, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.19026, [ 3, -0.30000, -0.08645], [ 3, 0.20000, 0.05764]], [ 0.45379, [ 3, -0.20000, -0.06162], [ 3, 0.20000, 0.06162]], [ 0.55995, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.38047, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipYawPitch")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ -0.47090, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, -0.00964], [ 3, 0.26667, 0.00771]], [ 0.00771, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.00000, [ 3, -0.26667, 0.00771], [ 3, 0.23333, -0.00675]], [ -0.65498, [ 3, -0.23333, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.49851, [ 3, -0.43333, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.85900, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.69639, [ 3, -0.30000, -0.09135], [ 3, 0.20000, 0.06090]], [ -0.40225, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.40225, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipRoll")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.04453, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.05833, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, 0.02189], [ 3, 0.23333, -0.01915]], [ -0.06478, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.54105, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.15498, [ 3, -0.26667, 0.11230], [ 3, 0.30000, -0.12633]], [ -0.17483, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.00925, [ 3, -0.30000, -0.07336], [ 3, 0.20000, 0.04891]], [ 0.19199, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.17453, [ 3, -0.20000, 0.00321], [ 3, 0.63333, -0.01015]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("LHipPitch")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.25008, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.17453, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.37434, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.17453, [ 3, -0.26667, 0.27599], [ 3, 0.23333, -0.24149]], [ -1.17808, [ 3, -0.23333, 0.27150], [ 3, 0.16667, -0.19393]], [ -1.57080, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.85746, [ 3, -0.26667, -0.23750], [ 3, 0.30000, 0.26719]], [ -0.05672, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.45556, [ 3, -0.30000, 0.15970], [ 3, 0.20000, -0.10647]], [ -0.85521, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.83599, [ 3, -0.20000, -0.01922], [ 3, 0.63333, 0.06087]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LKneePitch")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.09200, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.67552, [ 3, -0.33333, -0.35708], [ 3, 0.26667, 0.28566]], [ 2.02024, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.67552, [ 3, -0.26667, 0.16755], [ 3, 0.23333, -0.14661]], [ 1.07777, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 1.67552, [ 3, -0.16667, -0.13243], [ 3, 0.26667, 0.21189]], [ 2.11075, [ 3, -0.26667, -0.00135], [ 3, 0.30000, 0.00152]], [ 2.11227, [ 3, -0.30000, -0.00026], [ 3, 0.30000, 0.00026]], [ 2.11253, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ 2.11253, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 2.11253, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("LAnklePitch")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.82525, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.24435, [ 3, -0.33333, 0.23692], [ 3, 0.26667, -0.18953]], [ -0.45411, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.24435, [ 3, -0.26667, -0.20117], [ 3, 0.23333, 0.17602]], [ 0.67748, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.66323, [ 3, -0.16667, 0.01425], [ 3, 0.26667, -0.02280]], [ -0.45564, [ 3, -0.26667, 0.27681], [ 3, 0.30000, -0.31141]], [ -1.10145, [ 3, -0.30000, 0.08806], [ 3, 0.30000, -0.08806]], [ -1.18952, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ -1.18952, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -1.18952, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LAnkleRoll")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ -0.03371, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.03371, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, -0.03371], [ 3, 0.23333, 0.02949]], [ 0.17109, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.10472, [ 3, -0.16667, 0.07267], [ 3, 0.26667, -0.11627]], [ -0.39573, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.09967, [ 3, -0.30000, -0.07312], [ 3, 0.30000, 0.07312]], [ 0.04299, [ 3, -0.30000, -0.04355], [ 3, 0.20000, 0.02904]], [ 0.11810, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.08727, [ 3, -0.20000, 0.02037], [ 3, 0.63333, -0.06450]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipRoll")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ -0.14876, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.02297, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, -0.01509], [ 3, 0.23333, 0.01320]], [ 0.06190, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.54105, [ 3, -0.16667, 0.01080], [ 3, 0.26667, -0.01728]], [ -0.55833, [ 3, -0.26667, 0.01089], [ 3, 0.30000, -0.01225]], [ -0.61049, [ 3, -0.30000, 0.01137], [ 3, 0.30000, -0.01137]], [ -0.62657, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.29671, [ 3, -0.20000, -0.10152], [ 3, 0.20000, 0.10152]], [ -0.01745, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipPitch")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.18557, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.17453, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.28221, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.17453, [ 3, -0.26667, 0.25720], [ 3, 0.23333, -0.22505]], [ -1.16456, [ 3, -0.23333, 0.27150], [ 3, 0.16667, -0.19393]], [ -1.57080, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.52484, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -1.56012, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ -1.02974, [ 3, -0.30000, -0.13086], [ 3, 0.20000, 0.08724]], [ -0.90583, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.90583, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("RKneePitch")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.19179, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.67552, [ 3, -0.33333, -0.33975], [ 3, 0.26667, 0.27180]], [ 2.02645, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.67552, [ 3, -0.26667, 0.16579], [ 3, 0.23333, -0.14507]], [ 1.09386, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 1.67552, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.22111, [ 3, -0.26667, 0.09029], [ 3, 0.30000, -0.10158]], [ 1.09992, [ 3, -0.30000, 0.01607], [ 3, 0.30000, -0.01607]], [ 1.08385, [ 3, -0.30000, 0.01607], [ 3, 0.20000, -0.01071]], [ 0.87616, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 1.76278, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnklePitch")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.46638, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.24435, [ 3, -0.33333, 0.15567], [ 3, 0.26667, -0.12454]], [ -0.37425, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.24435, [ 3, -0.26667, -0.19407], [ 3, 0.23333, 0.16981]], [ 0.71737, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.66323, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.78545, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.78392, [ 3, -0.30000, 0.00153], [ 3, 0.30000, -0.00153]], [ 0.44157, [ 3, -0.30000, 0.05760], [ 3, 0.20000, -0.03840]], [ 0.40317, [ 3, -0.20000, 0.03840], [ 3, 0.20000, -0.03840]], [ -0.57945, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnkleRoll")
	times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
	keys.append([ [ 0.16725, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.03835], [ 3, 0.26667, -0.03068]], [ -0.03984, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.16084, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.10472, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00925, [ 3, -0.26667, 0.03182], [ 3, 0.30000, -0.03580]], [ -0.09813, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.44331, [ 3, -0.30000, -0.15555], [ 3, 0.20000, 0.10370]], [ 0.67960, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.27751, [ 3, -0.20000, 0.04933], [ 3, 0.63333, -0.15622]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	motion.angleInterpolationBezier(names, times, keys)

# Stand up from belly
def bellyToStand():
	names = list()
	times = list()
	keys = list()
	
	names.append("HeadYaw")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 0.17453, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.22689, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.28623, [ 3, -0.46667, -0.01333], [ 3, 0.36667, 0.01047]], [ 0.29671, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.49567, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.29671, [ 3, -0.23333, -0.07104], [ 3, 0.36667, 0.11164]], [ 0.05236, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.39095, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("HeadPitch")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ -0.57683, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.54768, [ 3, -0.33333, -0.02915], [ 3, 0.46667, 0.04081]], [ 0.10734, [ 3, -0.46667, -0.19834], [ 3, 0.36667, 0.15584]], [ 0.51487, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.38048, [ 3, -0.43333, 0.01726], [ 3, 0.23333, -0.00930]], [ 0.37119, [ 3, -0.23333, 0.00930], [ 3, 0.36667, -0.01461]], [ -0.10472, [ 3, -0.36667, 0.13827], [ 3, 0.43333, -0.16341]], [ -0.53387, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.15, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LShoulderPitch")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 0.08433, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.51146, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -1.25025, [ 3, -0.46667, -0.26120], [ 3, 0.36667, 0.20523]], [ 0.07206, [ 3, -0.36667, -0.38566], [ 3, 0.43333, 0.45578]], [ 1.27409, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.75573, [ 3, -0.23333, 0.00333], [ 3, 0.36667, -0.00524]], [ 0.75049, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 1.29154, [ 3, -0.43333, -0.15226], [ 3, 0.36667, 0.12884]], [ 1.59378, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LShoulderRoll")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 1.55390, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.01683, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.07666, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.07052, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.15643, [ 3, -0.43333, -0.08590], [ 3, 0.23333, 0.04626]], [ 0.93899, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.67719, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.84648, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.21779, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowYaw")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ -2.07694, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.58006, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -1.60461, [ 3, -0.46667, 0.02454], [ 3, 0.36667, -0.01928]], [ -1.78715, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.32695, [ 3, -0.43333, -0.11683], [ 3, 0.23333, 0.06291]], [ -1.24791, [ 3, -0.23333, -0.04593], [ 3, 0.36667, 0.07218]], [ -0.97260, [ 3, -0.36667, -0.01072], [ 3, 0.43333, 0.01267]], [ -0.95993, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -1.01862, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowRoll")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ -0.00873, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.35278, [ 3, -0.33333, 0.08741], [ 3, 0.46667, -0.12238]], [ -0.63810, [ 3, -0.46667, 0.09306], [ 3, 0.36667, -0.07312]], [ -0.85133, [ 3, -0.36667, 0.13944], [ 3, 0.43333, -0.16480]], [ -1.55083, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.73304, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.73653, [ 3, -0.36667, 0.00349], [ 3, 0.43333, -0.00413]], [ -1.15506, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.68105, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RShoulderPitch")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ -0.02757, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.51146, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -1.22256, [ 3, -0.46667, -0.23805], [ 3, 0.36667, 0.18704]], [ -0.23619, [ 3, -0.36667, -0.22007], [ 3, 0.43333, 0.26008]], [ 0.21787, [ 3, -0.43333, -0.14857], [ 3, 0.23333, 0.08000]], [ 0.44950, [ 3, -0.23333, -0.09028], [ 3, 0.36667, 0.14187]], [ 0.91431, [ 3, -0.36667, -0.03894], [ 3, 0.43333, 0.04602]], [ 0.96033, [ 3, -0.43333, -0.04602], [ 3, 0.36667, 0.03894]], [ 1.47882, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RShoulderRoll")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ -1.53558, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.19199, [ 3, -0.33333, -0.07793], [ 3, 0.46667, 0.10911]], [ -0.08288, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.08288, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.22707, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.18259, [ 3, -0.23333, -0.02831], [ 3, 0.36667, 0.04448]], [ -0.00870, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.13197, [ 3, -0.43333, 0.01994], [ 3, 0.36667, -0.01687]], [ -0.14884, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowYaw")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 2.07694, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.56157, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 1.61373, [ 3, -0.46667, -0.02319], [ 3, 0.36667, 0.01822]], [ 1.68582, [ 3, -0.36667, -0.05296], [ 3, 0.43333, 0.06259]], [ 1.96041, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 1.95121, [ 3, -0.23333, 0.00920], [ 3, 0.36667, -0.01445]], [ 0.66571, [ 3, -0.36667, 0.22845], [ 3, 0.43333, -0.26998]], [ 0.39573, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.90962, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowRoll")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 0.10472, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.38201, [ 3, -0.33333, -0.07367], [ 3, 0.46667, 0.10313]], [ 0.63512, [ 3, -0.46667, -0.21934], [ 3, 0.36667, 0.17234]], [ 1.55705, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.00870, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.00870, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.42343, [ 3, -0.36667, -0.09786], [ 3, 0.43333, 0.11566]], [ 0.64926, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipYawPitch")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ -0.03371, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.03491, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -0.43561, [ 3, -0.46667, 0.15197], [ 3, 0.36667, -0.11941]], [ -0.77923, [ 3, -0.36667, 0.09257], [ 3, 0.43333, -0.10940]], [ -1.04154, [ 3, -0.43333, 0.07932], [ 3, 0.23333, -0.04271]], [ -1.530, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -1, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.56754, [ 3, -0.43333, -0.16414], [ 3, 0.36667, 0.13889]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipRoll")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 0.06294, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.00158, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.37732, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.29755, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.29755, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.19486, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.12736, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipPitch")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 0.06140, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.06136], [ 3, 0.46667, -0.08590]], [ -1.56924, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -1.28085, [ 3, -0.36667, -0.08132], [ 3, 0.43333, 0.09611]], [ -1.03694, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -1.15966, [ 3, -0.23333, 0.01464], [ 3, 0.36667, -0.02301]], [ -1.18267, [ 3, -0.36667, 0.01687], [ 3, 0.43333, -0.01994]], [ -1.27011, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LKneePitch")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 0.12043, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.98968, [ 3, -0.33333, -0.08775], [ 3, 0.46667, 0.12285]], [ 2.11253, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.28221, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.40493, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.35738, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.71940, [ 3, -0.36667, -0.25311], [ 3, 0.43333, 0.29913]], [ 2.01409, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LAnklePitch")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 0.92189, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.02974, [ 3, -0.33333, 0.08628], [ 3, 0.46667, -0.12080]], [ -1.15054, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.21625, [ 3, -0.36667, -0.28428], [ 3, 0.43333, 0.33597]], [ 0.71020, [ 3, -0.43333, -0.15307], [ 3, 0.23333, 0.08242]], [ 0.92275, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.82525, [ 3, -0.36667, 0.09750], [ 3, 0.43333, -0.11522]], [ -0.50166, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LAnkleRoll")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ -0.00149, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -0.00149, [ 3, -0.46667, 0.00153], [ 3, 0.36667, -0.00121]], [ -0.45249, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.30062, [ 3, -0.43333, -0.07246], [ 3, 0.23333, 0.03901]], [ -0.11808, [ 3, -0.23333, -0.03361], [ 3, 0.36667, 0.05281]], [ -0.04138, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.12114, [ 3, -0.43333, 0.01632], [ 3, 0.36667, -0.01381]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipRoll")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 0.03142, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.00158, [ 3, -0.46667, -0.00153], [ 3, 0.36667, 0.00121]], [ 0.31144, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.25469, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.32065, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.22707, [ 3, -0.36667, 0.06047], [ 3, 0.43333, -0.07146]], [ -0.07512, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipPitch")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 0.07666, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.00004, [ 3, -0.33333, 0.07670], [ 3, 0.46667, -0.10738]], [ -1.57699, [ 3, -0.46667, 0.10738], [ 3, 0.36667, -0.08437]], [ -1.66136, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.19963, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -1.59847, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.32218, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.71028, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RKneePitch")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ -0.07819, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.98968, [ 3, -0.33333, -0.06900], [ 3, 0.46667, 0.09660]], [ 2.08628, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 1.74267, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 2.12019, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 2.12019, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 2.12019, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 2.12019, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnklePitch")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 0.92965, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.02974, [ 3, -0.33333, 0.07746], [ 3, 0.46667, -0.10844]], [ -1.13819, [ 3, -0.46667, 0.02925], [ 3, 0.36667, -0.02298]], [ -1.18645, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.18645, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.58901, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -1.18645, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.18645, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnkleRoll")
	times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
	keys.append([ [ 0.18850, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.00618, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.00456, [ 3, -0.36667, 0.01074], [ 3, 0.43333, -0.01269]], [ -0.09813, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.01376, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.09507, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.03532, [ 3, -0.43333, -0.02825], [ 3, 0.36667, 0.02390]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

	motion.angleInterpolationBezier(names, times, keys)
	
def stance():
	names = list()
	times = list()
	keys = list()
	
	names.append("LShoulderPitch")
	times.append([1.15])
	keys.append([ [ 1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LShoulderRoll")
	times.append([1.15])
	keys.append([ [ 0.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowYaw")
	times.append([1.15])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LElbowRoll")
	times.append([1.15])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipYawPitch")
	times.append([1])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipRoll")
	times.append([1.15])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LHipPitch")
	times.append([1])
	keys.append([ [ -1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("LKneePitch")
	times.append([1.15])
	keys.append([ [ 1.8, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("LAnklePitch")
	times.append([1.15])
	keys.append([ [ -0.75, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
	
	names.append("LAnkleRoll")
	times.append([1.15])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipRoll")
	times.append([1.15])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RHipPitch")
	times.append([1])
	keys.append([ [ -1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RKneePitch")
	times.append([1.15])
	keys.append([ [ 1.8, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnklePitch")
	times.append([1.15])
	keys.append([ [ -0.75, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RAnkleRoll")
	times.append([1.15])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])	
	
	names.append("RShoulderPitch")
	times.append([1.15])
	keys.append([ [ 1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RShoulderRoll")
	times.append([1.15])
	keys.append([ [ -0.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowYaw")
	times.append([1.15])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	names.append("RElbowRoll")
	times.append([1.15])
	keys.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

	motion.angleInterpolationBezier(names, times, keys)


	
def killWalk():
	motion.setWalkTargetVelocity(0,0,0,0)
	motion.walkTo(0,0,0)

def setWTV(x,y,t,f):
	motion.setWalkTargetVelocity(x,y,t,f)
	
# RETURNS
def getPose():
	return pose.getActualPoseAndTime()[0]
	
def isWalking():
	return motion.walkIsActive()

def getNormalPose():
	return [0, 0, 0, 0, 0, 0] == motion.getAngles("RLeg", False)

def getHead():
	return motion.getAngles(["HeadYaw", "HeadPitch"], True)
	
def getStance():	
	[ankle] = motion.getAngles("RAnklePitch", False)
	return (-0.7 >= ankle >= -0.8)

def stopTracker():
	track.stopTracker()

def startTracker():	
	track.startTracker()
	video.setParam(18,1)
	
def killAllTasks():
	normalPose()